
_PID_update:

;pid.c,11 :: 		void PID_update(float *RX_angle, float *SENSORS_angle, float *PID_output) {
;pid.c,19 :: 		for (cnt=0; cnt<3; cnt++) {
	CLRF        PID_update_cnt_L0+0 
L_PID_update0:
	MOVLW       3
	SUBWF       PID_update_cnt_L0+0, 0 
	BTFSC       STATUS+0, 0 
	GOTO        L_PID_update1
;pid.c,21 :: 		PID_err_proportional[cnt] = RX_angle[cnt] - SENSORS_angle[cnt];
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVF        R0, 0 
	ADDWF       FARG_PID_update_RX_angle+0, 0 
	MOVWF       FSR0L 
	MOVF        R1, 0 
	ADDWFC      FARG_PID_update_RX_angle+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R8 
	MOVF        POSTINC0+0, 0 
	MOVWF       R9 
	MOVF        POSTINC0+0, 0 
	MOVWF       R10 
	MOVF        POSTINC0+0, 0 
	MOVWF       R11 
	MOVF        R0, 0 
	ADDWF       FARG_PID_update_SENSORS_angle+0, 0 
	MOVWF       FSR2L 
	MOVF        R1, 0 
	ADDWFC      FARG_PID_update_SENSORS_angle+1, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R4 
	MOVF        POSTINC2+0, 0 
	MOVWF       R5 
	MOVF        POSTINC2+0, 0 
	MOVWF       R6 
	MOVF        POSTINC2+0, 0 
	MOVWF       R7 
	MOVF        R8, 0 
	MOVWF       R0 
	MOVF        R9, 0 
	MOVWF       R1 
	MOVF        R10, 0 
	MOVWF       R2 
	MOVF        R11, 0 
	MOVWF       R3 
	CALL        _Sub_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR1L
	MOVFF       FLOC__PID_update+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;pid.c,22 :: 		if((PID_err_proportional[cnt] > PID_err_integral_min) || (PID_err_proportional[cnt] < -PID_err_integral_min)) PID_err_integral[cnt] += PID_err_proportional[cnt]* PID_processTime;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        R0, 0 
	MOVWF       R4 
	MOVF        R1, 0 
	MOVWF       R5 
	MOVF        R2, 0 
	MOVWF       R6 
	MOVF        R3, 0 
	MOVWF       R7 
	MOVLW       10
	MOVWF       R0 
	MOVLW       215
	MOVWF       R1 
	MOVLW       35
	MOVWF       R2 
	MOVLW       120
	MOVWF       R3 
	CALL        _Compare_Double+0, 0
	MOVLW       1
	BTFSC       STATUS+0, 0 
	MOVLW       0
	MOVWF       R0 
	MOVF        R0, 1 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_update22
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       10
	MOVWF       R4 
	MOVLW       215
	MOVWF       R5 
	MOVLW       163
	MOVWF       R6 
	MOVLW       120
	MOVWF       R7 
	CALL        _Compare_Double+0, 0
	MOVLW       1
	BTFSC       STATUS+0, 0 
	MOVLW       0
	MOVWF       R0 
	MOVF        R0, 1 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_update22
	GOTO        L_PID_update5
L__PID_update22:
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_integral_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVLW       hi_addr(PID_update_PID_err_integral_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       111
	MOVWF       R4 
	MOVLW       18
	MOVWF       R5 
	MOVLW       3
	MOVWF       R6 
	MOVLW       118
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR0L
	MOVFF       FLOC__PID_update+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R4 
	MOVF        POSTINC0+0, 0 
	MOVWF       R5 
	MOVF        POSTINC0+0, 0 
	MOVWF       R6 
	MOVF        POSTINC0+0, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR1L
	MOVFF       FLOC__PID_update+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
L_PID_update5:
;pid.c,23 :: 		PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) / PID_processTime;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_derivative_L0+0
	ADDWF       R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVLW       hi_addr(PID_update_PID_err_derivative_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R8 
	MOVF        POSTINC0+0, 0 
	MOVWF       R9 
	MOVF        POSTINC0+0, 0 
	MOVWF       R10 
	MOVF        POSTINC0+0, 0 
	MOVWF       R11 
	MOVLW       PID_update_PID_err_proportional_old_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_old_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R4 
	MOVF        POSTINC2+0, 0 
	MOVWF       R5 
	MOVF        POSTINC2+0, 0 
	MOVWF       R6 
	MOVF        POSTINC2+0, 0 
	MOVWF       R7 
	MOVF        R8, 0 
	MOVWF       R0 
	MOVF        R9, 0 
	MOVWF       R1 
	MOVF        R10, 0 
	MOVWF       R2 
	MOVF        R11, 0 
	MOVWF       R3 
	CALL        _Sub_32x32_FP+0, 0
	MOVLW       111
	MOVWF       R4 
	MOVLW       18
	MOVWF       R5 
	MOVLW       3
	MOVWF       R6 
	MOVLW       118
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+0, FSR1L
	MOVFF       FLOC__PID_update+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;pid.c,24 :: 		PID_output[cnt] = (PID_Kp*PID_err_proportional[cnt]) + (PID_Ki*PID_err_integral[cnt]) + (PID_Kd*PID_err_derivative[cnt]);
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       FLOC__PID_update+6 
	MOVLW       0
	MOVWF       FLOC__PID_update+7 
	RLCF        FLOC__PID_update+6, 1 
	BCF         FLOC__PID_update+6, 0 
	RLCF        FLOC__PID_update+7, 1 
	RLCF        FLOC__PID_update+6, 1 
	BCF         FLOC__PID_update+6, 0 
	RLCF        FLOC__PID_update+7, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       FLOC__PID_update+6, 0 
	MOVWF       FLOC__PID_update+4 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      FLOC__PID_update+7, 0 
	MOVWF       FLOC__PID_update+5 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       FLOC__PID_update+6, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      FLOC__PID_update+7, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R0 
	MOVF        POSTINC2+0, 0 
	MOVWF       R1 
	MOVF        POSTINC2+0, 0 
	MOVWF       R2 
	MOVF        POSTINC2+0, 0 
	MOVWF       R3 
	MOVLW       51
	MOVWF       R4 
	MOVLW       51
	MOVWF       R5 
	MOVLW       51
	MOVWF       R6 
	MOVLW       126
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVF        R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVF        R2, 0 
	MOVWF       FLOC__PID_update+2 
	MOVF        R3, 0 
	MOVWF       FLOC__PID_update+3 
	MOVLW       PID_update_PID_err_integral_L0+0
	ADDWF       FLOC__PID_update+6, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_integral_L0+0)
	ADDWFC      FLOC__PID_update+7, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R0 
	MOVF        POSTINC2+0, 0 
	MOVWF       R1 
	MOVF        POSTINC2+0, 0 
	MOVWF       R2 
	MOVF        POSTINC2+0, 0 
	MOVWF       R3 
	MOVLW       154
	MOVWF       R4 
	MOVLW       153
	MOVWF       R5 
	MOVLW       25
	MOVWF       R6 
	MOVLW       124
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__PID_update+0, 0 
	MOVWF       R4 
	MOVF        FLOC__PID_update+1, 0 
	MOVWF       R5 
	MOVF        FLOC__PID_update+2, 0 
	MOVWF       R6 
	MOVF        FLOC__PID_update+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__PID_update+0 
	MOVF        R1, 0 
	MOVWF       FLOC__PID_update+1 
	MOVF        R2, 0 
	MOVWF       FLOC__PID_update+2 
	MOVF        R3, 0 
	MOVWF       FLOC__PID_update+3 
	MOVLW       PID_update_PID_err_derivative_L0+0
	ADDWF       FLOC__PID_update+6, 0 
	MOVWF       FSR2L 
	MOVLW       hi_addr(PID_update_PID_err_derivative_L0+0)
	ADDWFC      FLOC__PID_update+7, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R0 
	MOVF        POSTINC2+0, 0 
	MOVWF       R1 
	MOVF        POSTINC2+0, 0 
	MOVWF       R2 
	MOVF        POSTINC2+0, 0 
	MOVWF       R3 
	MOVLW       10
	MOVWF       R4 
	MOVLW       215
	MOVWF       R5 
	MOVLW       163
	MOVWF       R6 
	MOVLW       120
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__PID_update+0, 0 
	MOVWF       R4 
	MOVF        FLOC__PID_update+1, 0 
	MOVWF       R5 
	MOVF        FLOC__PID_update+2, 0 
	MOVWF       R6 
	MOVF        FLOC__PID_update+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVFF       FLOC__PID_update+4, FSR1L
	MOVFF       FLOC__PID_update+5, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;pid.c,26 :: 		if(PID_output[cnt] > PID_output_max) PID_output[cnt] = PID_output_max;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        R0, 0 
	MOVWF       R4 
	MOVF        R1, 0 
	MOVWF       R5 
	MOVF        R2, 0 
	MOVWF       R6 
	MOVF        R3, 0 
	MOVWF       R7 
	MOVLW       0
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	MOVLW       127
	MOVWF       R2 
	MOVLW       134
	MOVWF       R3 
	CALL        _Compare_Double+0, 0
	MOVLW       1
	BTFSC       STATUS+0, 0 
	MOVLW       0
	MOVWF       R0 
	MOVF        R0, 1 
	BTFSC       STATUS+0, 2 
	GOTO        L_PID_update6
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR1L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR1H 
	MOVLW       0
	MOVWF       POSTINC1+0 
	MOVLW       0
	MOVWF       POSTINC1+0 
	MOVLW       127
	MOVWF       POSTINC1+0 
	MOVLW       134
	MOVWF       POSTINC1+0 
	GOTO        L_PID_update7
L_PID_update6:
;pid.c,27 :: 		else if(PID_output[cnt] < PID_output_min) PID_output[cnt] = PID_output_min;
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	CLRF        R4 
	CLRF        R5 
	CLRF        R6 
	CLRF        R7 
	CALL        _Compare_Double+0, 0
	MOVLW       1
	BTFSC       STATUS+0, 0 
	MOVLW       0
	MOVWF       R0 
	MOVF        R0, 1 
	BTFSC       STATUS+0, 2 
	GOTO        L_PID_update8
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_output_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR1L 
	MOVLW       hi_addr(PID_update_PID_output_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR1H 
	CLRF        POSTINC1+0 
	CLRF        POSTINC1+0 
	CLRF        POSTINC1+0 
	CLRF        POSTINC1+0 
L_PID_update8:
L_PID_update7:
;pid.c,29 :: 		PID_err_proportional_old[cnt] = PID_err_proportional[cnt];
	MOVF        PID_update_cnt_L0+0, 0 
	MOVWF       R0 
	MOVLW       0
	MOVWF       R1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	RLCF        R0, 1 
	BCF         R0, 0 
	RLCF        R1, 1 
	MOVLW       PID_update_PID_err_proportional_old_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR1L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_old_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR1H 
	MOVLW       PID_update_PID_err_proportional_L0+0
	ADDWF       R0, 0 
	MOVWF       FSR0L 
	MOVLW       hi_addr(PID_update_PID_err_proportional_L0+0)
	ADDWFC      R1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       POSTINC1+0 
;pid.c,19 :: 		for (cnt=0; cnt<3; cnt++) {
	INCF        PID_update_cnt_L0+0, 1 
;pid.c,30 :: 		}
	GOTO        L_PID_update0
L_PID_update1:
;pid.c,31 :: 		}
	RETURN      0
; end of _PID_update

_PID_applyToMotors:

;pid.c,33 :: 		void PID_applyToMotors(float *PID_output, unsigned short *MOTORS_speed) {
;pid.c,37 :: 		for (cnt=0; cnt<4; cnt++) {
	CLRF        PID_applyToMotors_cnt_L0+0 
L_PID_applyToMotors9:
	MOVLW       4
	SUBWF       PID_applyToMotors_cnt_L0+0, 0 
	BTFSC       STATUS+0, 0 
	GOTO        L_PID_applyToMotors10
;pid.c,38 :: 		switch(cnt) {
	GOTO        L_PID_applyToMotors12
;pid.c,39 :: 		case 0: motors_temp = (int)(PID_output[0]*1.1547 + PID_output[1]*0.5); break;
L_PID_applyToMotors14:
	MOVFF       FARG_PID_applyToMotors_PID_output+0, FSR0L
	MOVFF       FARG_PID_applyToMotors_PID_output+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       54
	MOVWF       R4 
	MOVLW       205
	MOVWF       R5 
	MOVLW       19
	MOVWF       R6 
	MOVLW       127
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__PID_applyToMotors+0 
	MOVF        R1, 0 
	MOVWF       FLOC__PID_applyToMotors+1 
	MOVF        R2, 0 
	MOVWF       FLOC__PID_applyToMotors+2 
	MOVF        R3, 0 
	MOVWF       FLOC__PID_applyToMotors+3 
	MOVLW       4
	ADDWF       FARG_PID_applyToMotors_PID_output+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_PID_output+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       0
	MOVWF       R6 
	MOVLW       126
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__PID_applyToMotors+0, 0 
	MOVWF       R4 
	MOVF        FLOC__PID_applyToMotors+1, 0 
	MOVWF       R5 
	MOVF        FLOC__PID_applyToMotors+2, 0 
	MOVWF       R6 
	MOVF        FLOC__PID_applyToMotors+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+0 
	MOVF        R1, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+1 
	GOTO        L_PID_applyToMotors13
;pid.c,40 :: 		case 1: motors_temp = (int)(-(PID_output[0]*1.1547) + PID_output[1]*0.5); break;
L_PID_applyToMotors15:
	MOVFF       FARG_PID_applyToMotors_PID_output+0, FSR0L
	MOVFF       FARG_PID_applyToMotors_PID_output+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       54
	MOVWF       R4 
	MOVLW       205
	MOVWF       R5 
	MOVLW       19
	MOVWF       R6 
	MOVLW       127
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVLW       0
	XORWF       R0, 0 
	MOVWF       FLOC__PID_applyToMotors+0 
	MOVLW       0
	XORWF       R1, 0 
	MOVWF       FLOC__PID_applyToMotors+1 
	MOVLW       128
	XORWF       R2, 0 
	MOVWF       FLOC__PID_applyToMotors+2 
	MOVLW       0
	XORWF       R3, 0 
	MOVWF       FLOC__PID_applyToMotors+3 
	MOVLW       4
	ADDWF       FARG_PID_applyToMotors_PID_output+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_PID_output+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       0
	MOVWF       R6 
	MOVLW       126
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__PID_applyToMotors+0, 0 
	MOVWF       R4 
	MOVF        FLOC__PID_applyToMotors+1, 0 
	MOVWF       R5 
	MOVF        FLOC__PID_applyToMotors+2, 0 
	MOVWF       R6 
	MOVF        FLOC__PID_applyToMotors+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+0 
	MOVF        R1, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+1 
	GOTO        L_PID_applyToMotors13
;pid.c,41 :: 		case 2: motors_temp = (int)(-(PID_output[1])); break;
L_PID_applyToMotors16:
	MOVLW       4
	ADDWF       FARG_PID_applyToMotors_PID_output+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_PID_output+1, 0 
	MOVWF       FSR0H 
	MOVLW       0
	XORWF       POSTINC0+0, 0 
	MOVWF       R0 
	MOVLW       0
	XORWF       POSTINC0+0, 0 
	MOVWF       R1 
	MOVLW       128
	XORWF       POSTINC0+0, 0 
	MOVWF       R2 
	MOVLW       0
	XORWF       POSTINC0+0, 0 
	MOVWF       R3 
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+0 
	MOVF        R1, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+1 
	GOTO        L_PID_applyToMotors13
;pid.c,42 :: 		case 3: motors_temp = (int)(PID_output[2]); break;
L_PID_applyToMotors17:
	MOVLW       8
	ADDWF       FARG_PID_applyToMotors_PID_output+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_PID_output+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+0 
	MOVF        R1, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+1 
	GOTO        L_PID_applyToMotors13
;pid.c,43 :: 		}
L_PID_applyToMotors12:
	MOVF        PID_applyToMotors_cnt_L0+0, 0 
	XORLW       0
	BTFSC       STATUS+0, 2 
	GOTO        L_PID_applyToMotors14
	MOVF        PID_applyToMotors_cnt_L0+0, 0 
	XORLW       1
	BTFSC       STATUS+0, 2 
	GOTO        L_PID_applyToMotors15
	MOVF        PID_applyToMotors_cnt_L0+0, 0 
	XORLW       2
	BTFSC       STATUS+0, 2 
	GOTO        L_PID_applyToMotors16
	MOVF        PID_applyToMotors_cnt_L0+0, 0 
	XORLW       3
	BTFSC       STATUS+0, 2 
	GOTO        L_PID_applyToMotors17
L_PID_applyToMotors13:
;pid.c,44 :: 		motors_temp = (int)(PID_output[0]*1.1547 + PID_output[1]*0.5);
	MOVFF       FARG_PID_applyToMotors_PID_output+0, FSR0L
	MOVFF       FARG_PID_applyToMotors_PID_output+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       54
	MOVWF       R4 
	MOVLW       205
	MOVWF       R5 
	MOVLW       19
	MOVWF       R6 
	MOVLW       127
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__PID_applyToMotors+0 
	MOVF        R1, 0 
	MOVWF       FLOC__PID_applyToMotors+1 
	MOVF        R2, 0 
	MOVWF       FLOC__PID_applyToMotors+2 
	MOVF        R3, 0 
	MOVWF       FLOC__PID_applyToMotors+3 
	MOVLW       4
	ADDWF       FARG_PID_applyToMotors_PID_output+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_PID_output+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       0
	MOVWF       R4 
	MOVLW       0
	MOVWF       R5 
	MOVLW       0
	MOVWF       R6 
	MOVLW       126
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__PID_applyToMotors+0, 0 
	MOVWF       R4 
	MOVF        FLOC__PID_applyToMotors+1, 0 
	MOVWF       R5 
	MOVF        FLOC__PID_applyToMotors+2, 0 
	MOVWF       R6 
	MOVF        FLOC__PID_applyToMotors+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	CALL        _Double2Int+0, 0
	MOVF        R0, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+0 
	MOVF        R1, 0 
	MOVWF       PID_applyToMotors_motors_temp_L0+1 
;pid.c,45 :: 		if (motors_temp < MOTORS_min_speed) MOTORS_speed[cnt] = MOTORS_min_speed;
	MOVLW       128
	XORWF       R1, 0 
	MOVWF       R2 
	MOVLW       128
	SUBWF       R2, 0 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_applyToMotors23
	MOVLW       20
	SUBWF       R0, 0 
L__PID_applyToMotors23:
	BTFSC       STATUS+0, 0 
	GOTO        L_PID_applyToMotors18
	MOVF        PID_applyToMotors_cnt_L0+0, 0 
	ADDWF       FARG_PID_applyToMotors_MOTORS_speed+0, 0 
	MOVWF       FSR1L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_MOTORS_speed+1, 0 
	MOVWF       FSR1H 
	MOVLW       20
	MOVWF       POSTINC1+0 
	GOTO        L_PID_applyToMotors19
L_PID_applyToMotors18:
;pid.c,46 :: 		else if (motors_temp > MOTORS_max_speed) MOTORS_speed[cnt] = MOTORS_max_speed;
	MOVLW       128
	MOVWF       R0 
	MOVLW       128
	XORWF       PID_applyToMotors_motors_temp_L0+1, 0 
	SUBWF       R0, 0 
	BTFSS       STATUS+0, 2 
	GOTO        L__PID_applyToMotors24
	MOVF        PID_applyToMotors_motors_temp_L0+0, 0 
	SUBLW       250
L__PID_applyToMotors24:
	BTFSC       STATUS+0, 0 
	GOTO        L_PID_applyToMotors20
	MOVF        PID_applyToMotors_cnt_L0+0, 0 
	ADDWF       FARG_PID_applyToMotors_MOTORS_speed+0, 0 
	MOVWF       FSR1L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_MOTORS_speed+1, 0 
	MOVWF       FSR1H 
	MOVLW       250
	MOVWF       POSTINC1+0 
	GOTO        L_PID_applyToMotors21
L_PID_applyToMotors20:
;pid.c,47 :: 		else MOTORS_speed[cnt] = (unsigned short)(motors_temp);
	MOVF        PID_applyToMotors_cnt_L0+0, 0 
	ADDWF       FARG_PID_applyToMotors_MOTORS_speed+0, 0 
	MOVWF       FSR1L 
	MOVLW       0
	ADDWFC      FARG_PID_applyToMotors_MOTORS_speed+1, 0 
	MOVWF       FSR1H 
	MOVF        PID_applyToMotors_motors_temp_L0+0, 0 
	MOVWF       POSTINC1+0 
L_PID_applyToMotors21:
L_PID_applyToMotors19:
;pid.c,37 :: 		for (cnt=0; cnt<4; cnt++) {
	INCF        PID_applyToMotors_cnt_L0+0, 1 
;pid.c,48 :: 		}
	GOTO        L_PID_applyToMotors9
L_PID_applyToMotors10:
;pid.c,49 :: 		}
	RETURN      0
; end of _PID_applyToMotors
